"""
target detection
"""
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from swarm_msgs.msg import BoundingBox, BoundingBoxes
import os
import re
import numpy as np
import platform
import time
rospy.init_node('my_node_name', anonymous=True)
from YoloRknn import YoloRknn
yolo = YoloRknn('/home/orangepi/daji/src/topo_daji/det/scripts/best.rknn')
from IdManager import IdManager
#from swarm_msgs.msg._BoundingBox import BoundingBox
#from swarm_msgs.msg._BoundingBoxes import BoundingBoxes

# 加上收到照片后的时间戳
class image_converter:
  def __init__(self):
    self.image_pub = rospy.Publisher("/det/img",Image)
    self.bridge = CvBridge()
    self.image_sub = rospy.Subscriber("/TopoCam/img",Image,self.callback)
    self.bbox_pub = rospy.Publisher("/intercept/target",BoundingBoxes, queue_size=10)
    self.bboxes = BoundingBoxes()
    self.reid = IdManager()
    self.count = True
  def callback(self,data):
    try:
      self.count = not self.count
      if self.count:
        start_time = time.time() 
        self.bboxes.bounding_boxes = []
        cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        time1 = time.time()
        time_diff = (time1 - start_time) * 1000  # 转换为毫秒 
        yolo.det(cv_image)
        time2 = time.time()
        time_diff = (time2 - time1) * 1000  # 转换为毫秒 
        detections = []  
        for det_bbox in yolo.det_bboxes:
          bbox = BoundingBox()
          bbox.xmin=det_bbox['left']
          bbox.ymin=det_bbox['top']
          bbox.xmax=det_bbox['right']
          bbox.ymax=det_bbox['bottom']
          bbox.Class= det_bbox['id']
          print(det_bbox['id'])
          if bbox.Class == 'ball':
            self.bboxes.bounding_boxes.append(bbox)
        if len(self.bboxes.bounding_boxes)>0:
          print('---------->')
          print(self.bboxes)
          try:
            self.bbox_pub.publish(self.bboxes)
            print("pub finish")
          except Exception as e:  
            # 捕获其他所有异常，并打印出来  
            print(f"An error occurred: {e}")

        time3 = time.time()  
        time_diff = (time3 - time2) * 1000  # 转换为毫秒 
        #print("程序运行时间为：", time_diff, "毫秒") 
    except Exception as e:  
      # 捕获其他所有异常，并打印出来  
      print(f"An error occurred: {e}")
if __name__ == '__main__':
  ic = image_converter()
  rospy.spin()
    # for det_bbox in yolo.det_bboxes:
      
    #   detection = dict()
    #   detection['bbox'] = []
    #   detection['bbox'].append(det_bbox['left'])
    #   detection['bbox'].append(det_bbox['top'])
    #   detection['bbox'].append(det_bbox['right'])
    #   detection['bbox'].append(det_bbox['bottom'])
    #   detection['class'] = det_bbox['Class']
    #   detections.append(detection)
    #   #bbox.id = reid
    # #print("---------2")
    # frame_detections, new_ids = self.reid.match_objects(detections,time.time())  
    # print("---------3")
    # for det_ in frame_detections:
    #   print(det_)
    #   bbox = BoundingBox()
    #   bbox.xmin = det_['bbox'][0]
    #   bbox.xmax = det_['bbox'][2]
    #   bbox.ymin = det_['bbox'][3]
    #   bbox.ymax = det_['bbox'][1]
    #   bbox.Class = det_['class']
    #   bbox.id = det_['id']
    #   print(bbox.Class)
    #   bbox.timestamp = timestamp#是收到这张照片的时候的时间戳
    #   if bbox.Class == "ball":
    #     print("---------pub ballon")
    #     self.bboxes.append(bbox)
    
    # cv2.imshow("Image window", yolo.res_img)
    # cv2.waitKey(3)
